//   MeCab: Yet Another Part-of-Speech and Morphological Analyzer
//
//
//   lbfgs.c was ported from the FORTRAN code of lbfgs.m to C
//   using f2c converter
//
//   http://www.ece.northwestern.edu/~nocedal/lbfgs.html
//
//   Software for Large-scale Unconstrained Optimization
//   L-BFGS is a limited-memory quasi-Newton code for unconstrained
//   optimization.
//   The code has been developed at the Optimization Technology Center,
//   a joint venture of Argonne National Laboratory and Northwestern University.
//
//   Authors
//   Jorge Nocedal
//
//   References
//   - J. Nocedal. Updating Quasi-Newton Matrices with Limited Storage(1980),
//   Mathematics of Computation 35, pp. 773-782.
//   - D.C. Liu and J. Nocedal. On the Limited Memory Method for
//   Large Scale Optimization(1989),
//   Mathematical Programming B, 45, 3, pp. 503-528.
#include <cmath>
#include <iostream>
#include <numeric>
#include "lbfgs.h"
#include "common.h"

namespace SCRF {
static const double ftol = 1e-4;
static const double xtol = 1e-16;
static const double eps = 1e-7;
static const double lb3_1_gtol = 0.9;
static const double lb3_1_stpmin = 1e-20;
static const double lb3_1_stpmax = 1e20;
static const int lb3_1_mp = 6;
static const int lb3_1_lp = 6;

inline double sigma(double x) {
	if (x > 0) {
		return 1.0;
	} else if (x < 0) {
		return -1.0;
	}
	return 0.0;
}

inline double pi(double x, double y) {
	return sigma(x) == sigma(y) ? x : 0.0;
}

inline void daxpy_(int n, double da, const double *dx, double *dy) {
	for (int i = 0; i < n; ++i) {
		dy[i] += da * dx[i];
	}
}

inline double ddot_(int size, const double *dx, const double *dy) {
	return std::inner_product(dx, dx + size, dy, 0.0);
}

void mcstep(double *stx, double *fx, double *dx, double *sty, double *fy,
		double *dy, double *stp, double fp, double dp, int *brackt,
		double stpmin, double stpmax, int *info) {
	bool bound = true;
	double p, q, s, d1, d2, d3, r, gamma, theta, stpq, stpc, stpf;
	*info = 0;

	if (*brackt
			&& ((*stp <= std::min(*stx, *sty) || *stp >= std::max(*stx, *sty))
					|| *dx * (*stp - *stx) >= 0.0 || stpmax < stpmin)) {
		return;
	}

	double sgnd = dp * (*dx / std::abs(*dx));

	if (fp > *fx) {
		*info = 1;
		bound = true;
		theta = (*fx - fp) * 3 / (*stp - *stx) + *dx + dp;
		d1 = std::abs(theta);
		d2 = std::abs(*dx);
		d1 = std::max(d1, d2);
		d2 = std::abs(dp);
		s = std::max(d1, d2);
		d1 = theta / s;
		gamma = s * std::sqrt(d1 * d1 - *dx / s * (dp / s));
		if (*stp < *stx) {
			gamma = -gamma;
		}
		p = gamma - *dx + theta;
		q = gamma - *dx + gamma + dp;
		r = p / q;
		stpc = *stx + r * (*stp - *stx);
		stpq = *stx
				+ *dx / ((*fx - fp) / (*stp - *stx) + *dx) / 2 * (*stp - *stx);
		if ((d1 = stpc - *stx, std::abs(d1))
				< (d2 = stpq - *stx, std::abs(d2))) {
			stpf = stpc;
		} else {
			stpf = stpc + (stpq - stpc) / 2;
		}
		*brackt = true;
	} else if (sgnd < 0.0) {
		*info = 2;
		bound = false;
		theta = (*fx - fp) * 3 / (*stp - *stx) + *dx + dp;
		d1 = std::abs(theta);
		d2 = std::abs(*dx);
		d1 = std::max(d1, d2);
		d2 = std::abs(dp);
		s = std::max(d1, d2);
		d1 = theta / s;
		gamma = s * std::sqrt(d1 * d1 - *dx / s * (dp / s));
		if (*stp > *stx) {
			gamma = -gamma;
		}
		p = gamma - dp + theta;
		q = gamma - dp + gamma + *dx;
		r = p / q;
		stpc = *stp + r * (*stx - *stp);
		stpq = *stp + dp / (dp - *dx) * (*stx - *stp);
		if ((d1 = stpc - *stp, std::abs(d1))
				> (d2 = stpq - *stp, std::abs(d2))) {
			stpf = stpc;
		} else {
			stpf = stpq;
		}
		*brackt = true;
	} else if (std::abs(dp) < std::abs(*dx)) {
		*info = 3;
		bound = true;
		theta = (*fx - fp) * 3 / (*stp - *stx) + *dx + dp;
		d1 = std::abs(theta);
		d2 = std::abs(*dx);
		d1 = std::max(d1, d2);
		d2 = std::abs(dp);
		s = std::max(d1, d2);
		d3 = theta / s;
		d1 = 0.0;
		d2 = d3 * d3 - *dx / s * (dp / s);
		gamma = s * std::sqrt((std::max(d1, d2)));
		if (*stp > *stx) {
			gamma = -gamma;
		}
		p = gamma - dp + theta;
		q = gamma + (*dx - dp) + gamma;
		r = p / q;
		if (r < 0.0 && gamma != 0.0) {
			stpc = *stp + r * (*stx - *stp);
		} else if (*stp > *stx) {
			stpc = stpmax;
		} else {
			stpc = stpmin;
		}
		stpq = *stp + dp / (dp - *dx) * (*stx - *stp);
		if (*brackt) {
			if ((d1 = *stp - stpc, std::abs(d1))
					< (d2 = *stp - stpq, std::abs(d2))) {
				stpf = stpc;
			} else {
				stpf = stpq;
			}
		} else {
			if ((d1 = *stp - stpc, std::abs(d1))
					> (d2 = *stp - stpq, std::abs(d2))) {
				stpf = stpc;
			} else {
				stpf = stpq;
			}
		}
	} else {
		*info = 4;
		bound = false;
		if (*brackt) {
			theta = (fp - *fy) * 3 / (*sty - *stp) + *dy + dp;
			d1 = std::abs(theta);
			d2 = std::abs(*dy);
			d1 = std::max(d1, d2);
			d2 = std::abs(dp);
			s = std::max(d1, d2);
			d1 = theta / s;
			gamma = s * std::sqrt(d1 * d1 - *dy / s * (dp / s));
			if (*stp > *sty) {
				gamma = -gamma;
			}
			p = gamma - dp + theta;
			q = gamma - dp + gamma + *dy;
			r = p / q;
			stpc = *stp + r * (*sty - *stp);
			stpf = stpc;
		} else if (*stp > *stx) {
			stpf = stpmax;
		} else {
			stpf = stpmin;
		}
	}

	if (fp > *fx) {
		*sty = *stp;
		*fy = fp;
		*dy = dp;
	} else {
		if (sgnd < 0.0) {
			*sty = *stx;
			*fy = *fx;
			*dy = *dx;
		}
		*stx = *stp;
		*fx = fp;
		*dx = dp;
	}

	stpf = std::min(stpmax, stpf);
	stpf = std::max(stpmin, stpf);
	*stp = stpf;
	if (*brackt && bound) {
		if (*sty > *stx) {
			d1 = *stx + (*sty - *stx) * 0.66;
			*stp = std::min(d1, *stp);
		} else {
			d1 = *stx + (*sty - *stx) * 0.66;
			*stp = std::max(d1, *stp);
		}
	}

	return;
}
}

namespace SCRF {

class LBFGS::Mcsrch {
private:
	int infoc, stage1, brackt;
	double finit, dginit, dgtest, width, width1;
	double stx, fx, dgx, sty, fy, dgy, stmin, stmax;

public:
	Mcsrch() :
			infoc(0), stage1(0), brackt(0), finit(0.0), dginit(0.0), dgtest(
					0.0), width(0.0), width1(0.0), stx(0.0), fx(0.0), dgx(0.0), sty(
					0.0), fy(0.0), dgy(0.0), stmin(0.0), stmax(0.0) {
	}

	void mcsrch(int size, double *x, double f, const double *g, double *s,
			double *stp, int *info, int *nfev, double *wa, bool orthant,
			double C) {
		const double p5 = 0.5;
		const double p66 = 0.66;
		const double xtrapf = 4.0;
		const int maxfev = 20;

		/* Parameter adjustments */
		--wa;
		--s;
		--g;
		--x;

		if (*info == -1) {
			goto L45;
		}
		infoc = 1;

		if (size <= 0 || *stp <= 0.0) {
			return;
		}

		dginit = ddot_(size, &g[1], &s[1]);
		if (dginit >= 0.0) {
			return;
		}

		brackt = false;
		stage1 = true;
		*nfev = 0;
		finit = f;
		dgtest = ftol * dginit;
		width = lb3_1_stpmax - lb3_1_stpmin;
		width1 = width / p5;
		for (int j = 1; j <= size; ++j) {
			wa[j] = x[j];
		}

		stx = 0.0;
		fx = finit;
		dgx = dginit;
		sty = 0.0;
		fy = finit;
		dgy = dginit;

		while (true) {
			if (brackt) {
				stmin = std::min(stx, sty);
				stmax = std::max(stx, sty);
			} else {
				stmin = stx;
				stmax = *stp + xtrapf * (*stp - stx);
			}

			*stp = std::max(*stp, lb3_1_stpmin);
			*stp = std::min(*stp, lb3_1_stpmax);

			if ((brackt
					&& ((*stp <= stmin || *stp >= stmax) || *nfev >= maxfev - 1
							|| infoc == 0))
					|| (brackt && (stmax - stmin <= xtol * stmax))) {
				*stp = stx;
			}

			if (orthant) {
				for (int j = 1; j <= size; ++j) {
					double grad_neg = 0.0;
					double grad_pos = 0.0;
					double grad = 0.0;
					if (wa[j] == 0.0) {
						grad_neg = g[j] - 1.0 / C;
						grad_pos = g[j] + 1.0 / C;
					} else {
						grad_pos = grad_neg = g[j] + 1.0 * sigma(wa[j]) / C;
					}
					if (grad_neg > 0.0) {
						grad = grad_neg;
					} else if (grad_pos < 0.0) {
						grad = grad_pos;
					} else {
						grad = 0.0;
					}
					const double p = pi(s[j], -grad);
					const double xi =
							wa[j] == 0.0 ? sigma(-grad) : sigma(wa[j]);
					x[j] = pi(wa[j] + *stp * p, xi);
				}
			} else {
				for (int j = 1; j <= size; ++j) {
					x[j] = wa[j] + *stp * s[j];
				}
			}
			*info = -1;
			return;

			L45: *info = 0;
			++(*nfev);
			double dg = ddot_(size, &g[1], &s[1]);
			double ftest1 = finit + *stp * dgtest;

			if (brackt && ((*stp <= stmin || *stp >= stmax) || infoc == 0)) {
				*info = 6;
			}
			if (*stp == lb3_1_stpmax && f <= ftest1 && dg <= dgtest) {
				*info = 5;
			}
			if (*stp == lb3_1_stpmin && (f > ftest1 || dg >= dgtest)) {
				*info = 4;
			}
			if (*nfev >= maxfev) {
				*info = 3;
			}
			if (brackt && stmax - stmin <= xtol * stmax) {
				*info = 2;
			}
			if (f <= ftest1 && std::abs(dg) <= lb3_1_gtol * (-dginit)) {
				*info = 1;
			}

			if (*info != 0) {
				return;
			}

			if (stage1 && f <= ftest1
					&& dg >= std::min(ftol, lb3_1_gtol) * dginit) {
				stage1 = false;
			}

			if (stage1 && f <= fx && f > ftest1) {
				double fm = f - *stp * dgtest;
				double fxm = fx - stx * dgtest;
				double fym = fy - sty * dgtest;
				double dgm = dg - dgtest;
				double dgxm = dgx - dgtest;
				double dgym = dgy - dgtest;
				mcstep(&stx, &fxm, &dgxm, &sty, &fym, &dgym, stp, fm, dgm,
						&brackt, stmin, stmax, &infoc);
				fx = fxm + stx * dgtest;
				fy = fym + sty * dgtest;
				dgx = dgxm + dgtest;
				dgy = dgym + dgtest;
			} else {
				mcstep(&stx, &fx, &dgx, &sty, &fy, &dgy, stp, f, dg, &brackt,
						stmin, stmax, &infoc);
			}

			if (brackt) {
				double d1 = 0.0;
				if ((d1 = sty - stx, std::abs(d1)) >= p66 * width1) {
					*stp = stx + p5 * (sty - stx);
				}
				width1 = width;
				width = (d1 = sty - stx, std::abs(d1));
			}
		}

		return;
	}
};

void LBFGS::clear() {
	iflag_ = iscn = nfev = iycn = point = npt = iter = info = ispt = isyt =
			iypt = 0;
	stp = stp1 = 0.0;
	diag_.clear();
	w_.clear();
	delete mcsrch_;
	mcsrch_ = 0;
}

void LBFGS::lbfgs_optimize(int size, int msize, double *x, double f,
		const double *g, double *diag, double *w, bool orthant, double C,
		int *iflag) {
	double yy = 0.0;
	double ys = 0.0;
	int bound = 0;
	int cp = 0;

	--diag;
	--g;
	--x;
	--w;

	if (!mcsrch_) {
		mcsrch_ = new Mcsrch;
	}

	if (*iflag == 1) {
		goto L172;
	}
	if (*iflag == 2) {
		goto L100;
	}

	// initialization
	if (*iflag == 0) {
		point = 0;
		for (int i = 1; i <= size; ++i) {
			diag[i] = 1.0;
		}
		ispt = size + (msize << 1);
		iypt = ispt + size * msize;
		for (int i = 1; i <= size; ++i) {
			w[ispt + i] = -g[i] * diag[i];
		}
		stp1 = 1.0 / std::sqrt(ddot_(size, &g[1], &g[1]));
	}

	// MAIN ITERATION LOOP
	while (true) {
		++iter;
		info = 0;
		if (iter == 1)
			goto L165;
		if (iter > size)
			bound = size;

		// COMPUTE -H*G USING THE FORMULA GIVEN IN: Nocedal, J. 1980,
		// "Updating quasi-Newton matrices with limited storage",
		// Mathematics of Computation, Vol.24, No.151, pp. 773-782.
		ys = ddot_(size, &w[iypt + npt + 1], &w[ispt + npt + 1]);
		yy = ddot_(size, &w[iypt + npt + 1], &w[iypt + npt + 1]);
		for (int i = 1; i <= size; ++i) {
			diag[i] = ys / yy;
		}

		L100: cp = point;
		if (point == 0)
			cp = msize;
		w[size + cp] = 1.0 / ys;

		for (int i = 1; i <= size; ++i) {
			w[i] = -g[i];
		}

		bound = std::min(iter - 1, msize);

		cp = point;
		for (int i = 1; i <= bound; ++i) {
			--cp;
			if (cp == -1)
				cp = msize - 1;
			double sq = ddot_(size, &w[ispt + cp * size + 1], &w[1]);
			int inmc = size + msize + cp + 1;
			iycn = iypt + cp * size;
			w[inmc] = w[size + cp + 1] * sq;
			double d = -w[inmc];
			daxpy_(size, d, &w[iycn + 1], &w[1]);
		}

		for (int i = 1; i <= size; ++i) {
			w[i] = diag[i] * w[i];
		}

		for (int i = 1; i <= bound; ++i) {
			double yr = ddot_(size, &w[iypt + cp * size + 1], &w[1]);
			double beta = w[size + cp + 1] * yr;
			int inmc = size + msize + cp + 1;
			beta = w[inmc] - beta;
			iscn = ispt + cp * size;
			daxpy_(size, beta, &w[iscn + 1], &w[1]);
			++cp;
			if (cp == msize) {
				cp = 0;
			}
		}

		// STORE THE NEW SEARCH DIRECTION
		for (int i = 1; i <= size; ++i) {
			w[ispt + point * size + i] = w[i];
		}

		L165:
		// OBTAIN THE ONE-DIMENSIONAL MINIMIZER OF THE FUNCTION
		// BY USING THE LINE SEARCH ROUTINE MCSRCH
		nfev = 0;
		stp = 1.0;
		if (iter == 1) {
			stp = stp1;
		}
		for (int i = 1; i <= size; ++i) {
			w[i] = g[i];
		}

		L172: mcsrch_->mcsrch(size, &x[1], f, &g[1],
				&w[ispt + point * size + 1], &stp, &info, &nfev, &diag[1],
				orthant, C);
		if (info == -1) {
			*iflag = 1;  // next value
			return;
		}
		if (info != 1) {
			std::cerr << "The line search routine mcsrch failed: error code:"
					<< info << std::endl;
			*iflag = -1;
			return;
		}

		// COMPUTE THE NEW STEP AND GRADIENT CHANGE
		npt = point * size;
		for (int i = 1; i <= size; ++i) {
			w[ispt + npt + i] = stp * w[ispt + npt + i];
			w[iypt + npt + i] = g[i] - w[i];
		}
		++point;
		if (point == msize) {
			point = 0;
		}

		double gnorm = std::sqrt(ddot_(size, &g[1], &g[1]));
		double xnorm = std::max(1.0, std::sqrt(ddot_(size, &x[1], &x[1])));
		if (gnorm / xnorm <= eps) {
			*iflag = 0;  // OK terminated
			return;
		}
	}
}
}
